/*
 * LineTracker.c
 *
 *  Created on: 2012.11.17.
 *      Author: Tibi
 */

#include "LineTracker.h"

__IO uint16_t LT_Values[24];
LineData LineArray[3];
uint8_t NumOfLine = 0;


void SPI2_Init(void)
{
	//SCK:		PB13 out
	//MOSI: 	PB15 out

	GPIO_InitTypeDef GPIO_InitStructure;
	SPI_InitTypeDef  SPI_InitStructure;

	RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE);
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);

	GPIO_PinAFConfig(GPIOB, GPIO_PinSource13, GPIO_AF_SPI2);	//SCK
	GPIO_PinAFConfig(GPIOB, GPIO_PinSource15, GPIO_AF_SPI2);	//MOSI

	GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_DOWN;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;
	GPIO_Init(GPIOB, &GPIO_InitStructure);

	/* SPI configuration */
	SPI_I2S_DeInit(SPI2);
	SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
	SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
	SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
	SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;
	SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
	SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8;
	SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
	SPI_InitStructure.SPI_CRCPolynomial = 7;
	SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
	SPI_Init(SPI2, &SPI_InitStructure);

	/* Enable SPI2  */
	SPI_SSOutputCmd(SPI2,ENABLE);
	SPI_Cmd(SPI2, ENABLE);
}

void LT_Pins_Init()
{
	GPIO_InitTypeDef GPIO_InitStructure;

	/* Enable GPIO clocks */
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOC, ENABLE);

	/* Configure GPIO PINs */
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;

	GPIO_InitStructure.GPIO_Pin = LT_LE_PIN;
	GPIO_Init(LT_LE_PORT, &GPIO_InitStructure);

	GPIO_InitStructure.GPIO_Pin = LT_OEN_PIN;
	GPIO_Init(LT_OEN_PORT, &GPIO_InitStructure);

	GPIO_InitStructure.GPIO_Pin = LT_MUXA_PIN;
	GPIO_Init(LT_MUX_PORT, &GPIO_InitStructure);

	GPIO_InitStructure.GPIO_Pin = LT_MUXB_PIN;
	GPIO_Init(LT_MUX_PORT, &GPIO_InitStructure);

	GPIO_InitStructure.GPIO_Pin = LT_MUXC_PIN;
	GPIO_Init(LT_MUX_PORT, &GPIO_InitStructure);

	GPIO_InitStructure.GPIO_Pin = LT_MUXINH_PIN;
	GPIO_Init(LT_MUXINH_PORT, &GPIO_InitStructure);

	GPIO_SetBits(LT_OEN_PORT, LT_OEN_PIN);
	GPIO_SetBits(LT_MUXINH_PORT, LT_MUXINH_PIN);
	GPIO_ResetBits(LT_LE_PORT, LT_LE_PIN);

	/* Configure ADC Channel1,2,3 pin as analog input ******************************/
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL ;
	GPIO_Init(GPIOA, &GPIO_InitStructure);

}



void LT_Init(void)
{
	uint8_t i;
	SPI2_Init();
	LT_Pins_Init();
	ADC_Initialize();
	for(i = 0; i < 3; i++)
	{
		LineArray[i].linepos = 0xFF;
		LineArray[i].valid = 0;
	}
}

void LT_SetMuxABC(uint8_t abc)
{
	abc &= 0x7;
	GPIO_SetBits(LT_MUX_PORT, abc);
	GPIO_ResetBits(LT_MUX_PORT, (~abc) & 0x07);
}

void LT_Write(uint32_t data)
{
	uint8_t byte = data & 0xff;
	while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
	SPI_I2S_SendData(SPI2, byte );
	//while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);

	byte = (data >> 8) & 0xff;
	while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
	SPI_I2S_SendData(SPI2, byte );
	//while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);

	byte = (data >> 16) & 0xff;
	while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
	SPI_I2S_SendData(SPI2, byte );
	while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_BSY) == SET);

	GPIO_SetBits(LT_LE_PORT, LT_LE_PIN);
	//for(byte=0; byte < 10; byte++);
	byte++;byte++;byte++;byte++;byte++;
	GPIO_ResetBits(LT_LE_PORT, LT_LE_PIN);
}

void LT_SetDMATarget(DMA_Stream_TypeDef* DMAy_Streamx, uint32_t MemoryBaseAddr)
{
	DMA_Cmd(DMAy_Streamx, DISABLE);
	DMA_MemoryTargetConfig(DMAy_Streamx, MemoryBaseAddr, DMA_Memory_0);
	DMA_Cmd(DMAy_Streamx, ENABLE);
}

void LT_RefreshValues(void)
{
	int j = 0;
	uint8_t i=0;

	for(; i<24; i++) LT_Values[i] = 0;	//clearing memory

	GPIO_ResetBits(LT_MUXINH_PORT, LT_MUXINH_PIN);

	for(i = 0; i < 4; i++)
	{
		LT_Write(0x111111 << i);
		GPIO_ResetBits(LT_OEN_PORT, LT_OEN_PIN);
		while (j != 5000) j++;
		j = 0;
		LT_SetMuxABC(i);
		LT_SetDMATarget(DMA2_Stream0, (uint32_t)&LT_Values[i]);
		LT_SetDMATarget(DMA2_Stream2, (uint32_t)&LT_Values[i + 8]);
		LT_SetDMATarget(DMA2_Stream1, (uint32_t)&LT_Values[i + 16]);

		DMA_ClearFlag(DMA2_Stream1, DMA_FLAG_TCIF1);
		ADC_SoftwareStartConv(ADC1);
		ADC_SoftwareStartConv(ADC2);
		ADC_SoftwareStartConv(ADC3);
		while(DMA_GetFlagStatus(DMA2_Stream1, DMA_FLAG_TCIF1) == RESET);	//waiting for DMA transfer from ADC3

		LT_SetMuxABC(i + 4);
		LT_SetDMATarget(DMA2_Stream0, (uint32_t)&LT_Values[i + 4]);
		LT_SetDMATarget(DMA2_Stream2, (uint32_t)&LT_Values[i + 12]);
		LT_SetDMATarget(DMA2_Stream1, (uint32_t)&LT_Values[i + 20]);

		DMA_ClearFlag(DMA2_Stream1, DMA_FLAG_TCIF1);
		ADC_SoftwareStartConv(ADC1);
		ADC_SoftwareStartConv(ADC2);
		ADC_SoftwareStartConv(ADC3);
		while(DMA_GetFlagStatus(DMA2_Stream1, DMA_FLAG_TCIF1) == RESET);	//waiting for DMA transfer from ADC3
	}

	GPIO_SetBits(LT_OEN_PORT, LT_OEN_PIN);
	GPIO_SetBits(LT_MUXINH_PORT, LT_MUXINH_PIN);
}

void GetLTValues(void){
	uint8_t i;
	ADC_ChangeChannel(ADC_Channel_1, ADC_Channel_2, ADC_Channel_3);
	LT_RefreshValues();
	for(i=0; i<24; i++) values[i] = (LT_Values[i]>>4) * 3.0 / 0xfff;
	LT_GetLineData();
}

void LT_GetLineData()
{
	uint16_t lminValue = 65535;
	uint16_t lmaxValue = 0;
	uint16_t lthreshold = 65535;
	uint8_t  lsensIndices[24];
	uint8_t  lActiveSensNum = 0;
	uint8_t  llineDetected = 0;
	uint8_t i,j;
	float lsum = 0;
	float lDiv = 1;
	/* Clear previous values */
	NumOfLine = 0;
	for(i = 0; i < 3; i++)
	{
		LineArray[i].linepos = 0xFF;
		LineArray[i].valid = 0;
	}
	/* Find minimum and maximum*/
	for(i = 0; i < NUM_OF_SENSORS ; i++)
	{
		if(lminValue > LT_Values[i])
		{
			lminValue = LT_Values[i];
		}
		if(lmaxValue < LT_Values[i])
		{
			lmaxValue = LT_Values[i];
		}
	}
	/* Calculate threshold */
	lthreshold = ((lmaxValue - lminValue) >> 3) + lminValue;
	if(lthreshold > 15000)
	{
		lthreshold = 65535;
	}
	/* Check values against calculated threshold*/
	for(i = 0; i < NUM_OF_SENSORS; i++)
	{
		if(LT_Values[i] < lthreshold)
		{
			lsensIndices[lActiveSensNum] = i;
			lActiveSensNum++;
			llineDetected = 1;
		} else if(llineDetected == 1)
		{
			llineDetected = 0;
			for(j = 0;j < lActiveSensNum;j++)
			{
				lsum += (float)((lthreshold-LT_Values[lsensIndices[j]])*lsensIndices[j]);
				lDiv += (float)(lthreshold - LT_Values[lsensIndices[j]]);
			}
			lActiveSensNum = 0;
			LineArray[NumOfLine].linepos = lsum / lDiv;
			LineArray[NumOfLine].valid = 1;
			NumOfLine++;
		}
	}


}
